# dic = {}
# line = input()
# n = int(line)
# for _ in range(n):
#     line = input().split(' ')
#     index,value = int(line[0]), int(line[1])
#     if index not in dic:
#         dic[index] = value
#     else:
#         dic[index] += value
# sorted(dic.items(), key=lambda x: x[0])
# for item in dic:
#     print(item[0],item[1])

#     line = input().strip()
#     p = []
#     res = [0]*len(line)

#     for i,item in enumerate(line):
#         if item.isalpha():
#             p.append(item)
#         else:
#             res[i] = item
            
#     q = sorted(p, key = lambda x: x.lower())
#     q.reverse()
#     for i in range(len(line)):
#         if not res[i]:
#             res[i] = q.pop()
            
#     print(''.join(res))
from scipy.stats import stats
import numpy as np
from loop_closure_f import *
import math
import matplotlib.pyplot as plt
a = np.array([[1,2,0],[1,2,0]])

# print(np.average([1,2,0]))

absDisToLine1 = np.zeros(len(a[0,:]) - 2) #首尾距离为0，不用算
print(absDisToLine1)
s1 = np.array([[ c for c in range(100)],[math.sin(i/10) for i in range(100)]]).T
s2 = s1 + [0,10]
plt.plot(s1[:,0],s1[:,1],marker=',')
plt.plot(s2[:,0],s2[:,1],marker=',')
plt.show()

absDisToLine1 = calDistoLine(s1)
absDisToLine2 = calDistoLine(s2)
print(absDisToLine1)
r,p_value = stats.pearsonr(absDisToLine1,absDisToLine2)
print(r)
plt.plot(absDisToLine1,marker=',')
plt.show()
plt.plot(absDisToLine2,marker=',')
plt.show()


# res = sequenceMatch(s1,s2)
# plt.plot(x,s1,marker=',')
# plt.plot(x[res:res+len(s2)],s1[res:res+len(s2)],marker=',')
# plt.plot(x[s2_start_index:],s2,marker=',')
# plt.show()
# print(res)

# a = np.array( [1,2,3])
# res = np.sqrt(a.T*a).T
# print(res)

# from compute_f import *
# a = np.array([1,2,0])
# b = np.array([2,2,0])
# print(v2t(a))
# print(np.linalg.inv(v2t(a)))
# A = np.dot(np.linalg.inv(v2t(a)),v2t(b))
# print(A)
# B = t2v(A)
# print(B)
# line = 'aaaabb'
# res = []
# for i in range(len(line)-1,-1):
#     res.append(line[i])
# print(''.join(res))
# a = 1

# f = open("DK.g2o","w")
# f.write('VERTEX_SE2 %d'%(a))   
# f.write('VERTEX_SE2 %d'%(a))   

from curve_line_segmentation_f import grid_pos
import numpy as np
pos_g = np.zeros(3,2)
pos_g = np.array([[0,0,0,0,-350],[0,0,10,0,10]])
print(grid_pos(pos_g))